#!/usr/bin/env python

import roslib; roslib.load_manifest('DelphinROSv2')
import rospy
import smach
import smach_ros
import time


class GoToDepth(smach.State):
	def __init__(self, lib, demand, tolerance, timeout):
		smach.State.__init__(self, outcomes=['succeeded','aborted','preempted'])
                self.__controller = lib
                self.__demand = demand
                self.__tolerance = tolerance
                self.__timeout = timeout
  		
	def execute(self,userdata):
		time_zero = time.time()

                self.__controller.setDepth(self.__demand)
                
                while (time.time()-time_zero < self.__timeout) and self.__controller.getBackSeatErrorFlag() == 0:
                    if abs(self.__controller.getDepth() - self.__demand) <= self.__tolerance:  
                        # TO DO:
                        # need to wait for depth to be roughly constant (at present overshooting will be considered a success)
                        # check pitch angle is within sensible limits
                        return 'succeeded'
                    
                if self.__controller.getBackSeatErrorFlag() == 1:
                    return 'preempted'
                else:
                    return 'aborted'  